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Abstract:  In  this  paper,  a  unique  camera  mapping  be¬ 
tween  the  desired  camera  feature  vector  and  the  desired 
camera  pose  (i.e.,  the  position  and  orientation)  is  in¬ 
vestigated  to  develop  a  measurable  image  Jacobian-like 
matrix.  An  image-space  path  planner  is  then  proposed 
to  generate  a  desired  image  trajectory  based  on  this  mea¬ 
surable  image  Jacobian-like  matrix  and  an  image  space 
navigation  function  (NF)  (i.e.,  a  special  potential  field 
function)  while  satisfying  rigid  body  constraints.  An 
adaptive,  homography-based  visual  servo  tracking  con¬ 
troller  is  then  developed  to  navigate  the  position  and  ori¬ 
entation  of  a  camera  held  by  the  end- effector  of  a  robot 
manipulator  to  a  goal  position  and  orientation  along  the 
desired  image-space  trajectory  while  ensuring  the  target 
points  remain  visible  (i.e.,  the  target  points  avoid  self¬ 
occlusion  and  remain  in  the  field- of-view  (FOV))  under 
certain  technical  restrictions.  The  self- occlusion  prob¬ 
lem  is  also  discussed.  Due  to  the  inherent  nonlinear 
nature  of  the  problem  and  the  lack  of  depth  information 
from  a  monocular  system,  a  Lyapunov-based  analysis  is 
used  to  analyze  the  path  planner  and  the  adaptive  con¬ 
troller.  Simulation  results  are  provided  to  illustrate  the 
performance  of  the  proposed  approach. 

1  Introduction 

There  is  significant  motivation  to  provide  improved  au¬ 
tonomy  for  robotic  systems.  In  part,  this  motivation 
has  lead  researchers  to  investigate  the  basic  science 
challenges  leading  to  the  development  of  visual  servo 
controllers  as  a  means  to  provide  improved  robot  au¬ 
tonomy.  In  general,  visual  servo  controllers  can  be 
divided  into  position-based  visual  servo  (PBVS)  con¬ 
trol,  image-based  visual  servo  (IBVS),  and  hybrid  ap¬ 
proaches.  PBVS  is  based  on  the  idea  of  using  a  vision 
system  to  reconstruct  the  Euclidean-space  and  then  de¬ 
veloping  the  servo  controller  on  the  reconstructed  in¬ 
formation.  A  well  known  issue  with  this  strategy  is 
that  the  target  object  may  exit  the  camera  field-of-view 
(FOV).  IBVS  control  is  based  on  the  idea  of  directly 

1This  research  was  supported  in  part  by  U.S.  NSF  Grant  DMI- 
9457967,  ONR  Grant  N00014-99-1-0589,  a  DOC  Grant,  and  an 
ARO  Automotive  Center  Grant  at  Clemson  University,  and  in 
part  by  AFOSR  contract  number  F49620-03-1-0381  at  the  Uni¬ 
versity  of  Florida. 


servoing  on  the  image-space  information,  with  reported 
advantages  of  increased  robustness  to  camera  calibra¬ 
tion  and  improved  capabilities  to  ensure  the  target  re¬ 
mains  visible.  Even  for  IBVS  controllers  that  are  formu¬ 
lated  as  regulation  controllers,  if  the  initial  error  is  large 
then  excessive  control  action  and  transient  response  can 
cause  the  target  to  leave  the  FOV,  and  may  lead  to  tra¬ 
jectories  that  are  not  physically  valid  or  optimal  due 
to  the  nonlinearities  and  potential  singularities  with  as¬ 
sociated  the  transformation  between  the  image  space 
and  the  Euclidean-space  [2].  For  a  review  of  IBVS  and 
PBVS  controllers  see  [19]. 

In  light  of  the  characteristics  of  IBVS  and  PBVS,  several 
researchers  have  recently  explored  hybrid  approaches. 
For  example,  homography-based  visual  servo  control 
techniques  (coined  2.5D  controllers)  have  been  recently 
developed  in  a  series  of  papers  by  Malis  and  Chaumette 
(e.g.,  [1],  [25],  [26]).  The  homography-based  approach 
exploits  a  combination  of  reconstructed  Euclidean  in¬ 
formation  and  image-space  information  in  the  control 
design.  The  Euclidean  information  is  reconstructed  by 
decoupling  the  interaction  between  translational  and  ro¬ 
tational  components  of  a  homography  matrix.  As  stated 
in  [25],  some  advantages  of  this  methodology  over  the 
aforementioned  IBVS  and  PBVS  approaches  are  that  an 
accurate  Euclidean  model  of  the  environment  (or  tar¬ 
get  object)  is  not  required,  and  potential  singularities 
in  the  image- Jacobian  are  eliminated  (i.e.,  the  image- 
Jacobian  for  homography-based  visual  servo  controllers 
is  typically  triangular).  Motivated  by  the  advantages 
of  the  homography-based  strategy,  several  researchers 
have  recently  developed  various  regulation  controllers 
for  robot  manipulators  (see  [5],  [7],  and  [10]). 

While  homography-based  approaches  exploit  the  advan¬ 
tages  of  IBVS  and  PBVS,  a  common  problem  with 
all  the  aforementioned  approaches  is  the  inability  to 
achieve  the  control  objective  while  ensuring  the  target 
features  remain  visible.  To  address  this  issue,  Mezouar 
and  Chaumette  developed  a  path-following  IBVS  algo¬ 
rithm  in  [28]  where  the  path  to  a  goal  point  is  gener¬ 
ated  via  a  potential  function  that  incorporates  motion 
constraints;  however,  as  stated  in  [28],  local  minima  as¬ 
sociated  with  traditional  potential  functions  may  exist. 


Using  a  specialized  potential  function  (coined  a  navi¬ 
gation  function  (NF))  originally  proposed  in  [23]  and 
[33],  Cowan  et  al.  developed  a  hybrid  position/image- 
space  controller  that  forces  a  manipulator  to  a  desired 
setpoint  while  ensuring  the  object  remains  visible  (i.e. , 
the  NF  ensures  no  local  minima)  and  by  avoiding  pit- 
falls  such  as  self-occlusion  [9].  However,  as  stated  in 
[28],  this  approach  requires  the  complete  knowledge  of 
the  space  topology  and  requires  an  object  model.  In 
[17],  Gans  and  Hutchinson  developed  a  strategy  that 
switches  between  an  IBVS  and  a  PBVS  controller  to 
ensure  asymptotic  stability  of  the  position  and  orienta¬ 
tion  (i.e.,  pose)  in  the  Euclidean  and  image-space.  An 
image-space  based  follow-the-leader  application  for  mo¬ 
bile  robots  was  developed  in  [8]  that  exploits  an  image- 
space  NF.  Specifically,  an  input/output  feedback  lin¬ 
earization  technique  is  applied  to  the  mobile  robot  kine¬ 
matic  model  to  yield  a  controller  that  yields  “string  sta¬ 
bility”  [15].  Without  a  feedforward  component,  the  con¬ 
troller  in  [8]  yields  an  approximate  “input-to-formation” 
stability  (i.e.,  a  local,  linear  exponential  system  with  a 
bounded  disturbance).  A  NF  based  approach  to  the 
follow-the-leader  problem  for  a  group  of  fully  actuated 
holonomic  mobile  robots  is  considered  in  [30]  where  con¬ 
figuration  based  constraints  are  developed  to  ensure  the 
robot  edges  remain  in  the  sight  of  an  omnidirectional 
camera.  While  a  Lyapunov-based  analysis  is  provided 
in  [30]  to  ensure  that  the  NF  decreases  to  the  goal  posi¬ 
tion,  the  stability  of  the  overall  system  is  not  examined. 

Motivated  by  the  image  space  navigation  function  de¬ 
veloped  in  [9],  an  off-line  desired  image  trajectory  gen¬ 
erator  is  proposed  based  on  a  new  image  Jacobian- like 
matrix  for  the  monocular,  camera-in-hand  problem. 
This  approach  generates  a  desired  camera  pose  trajec¬ 
tory  that  moves  the  camera  from  the  initial  camera  pose 
to  a  goal  camera  pose  while  ensuring  that  all  the  feature 
points  of  the  object  remain  visible  under  certain  tech¬ 
nical  restrictions.  To  develop  a  desired  camera  pose 
trajectory  that  ensures  all  feature  points  remain  visi¬ 
ble,  a  unique  relationship  is  formulated  between  the  de¬ 
sired  image  feature  vector  and  the  desired  camera  pose. 
The  resulting  image  Jacobian-like  matrix  is  related  to 
the  camera  pose,  rather  than  the  camera  velocity  as  in 
other  approaches  [2] .  Motivation  for  the  development  of 
this  relationship  is  that  the  resulting  image  Jacobian- 
like  matrix  is  measurable,  and  hence,  does  not  suffer 
from  the  lack  of  robustness  associated  with  estimation 
based  methods.  Further  more,  the  desired  image  gen¬ 
erated  with  this  image  Jacobian-like  matrix  will  satisfy 
rigid  body  constraints  automatically  (The  terminology, 
rigid  body  contraints,  in  this  paper  is  utilized  to  denote 
the  constraints  for  the  image  feature  vector  that  the  fea¬ 
ture  points  have  a  fixed  relative  position  to  each  other 
in  Eucledean  space) .  Building  on  our  recent  research  in 
[5] ,  an  adaptive  homography  based  visual  tracking  con- 


Figure  1:  Coordinate  frame  relationships 


troller  is  then  developed  to  ensure  that  the  actual  cam¬ 
era  pose  tracks  the  desired  camera  pose  trajectory  (i.e., 
the  actual  features  track  the  desired  feature  point  tra¬ 
jectory)  despite  the  fact  that  time-varying  depth  from 
the  camera  to  the  reference  image  plane  is  not  measur¬ 
able  from  the  monocular  camera  system.  Based  on  the 
analysis  of  the  homography  based  controller,  bounds  are 
developed  that  can  be  used  to  ensure  that  the  actual  im¬ 
age  features  also  remain  visible  under  certain  technical 
restrictions.  A  Lyapunov-based  analysis  is  provided  to 
support  the  claims  for  the  path  planner  and  to  analyze 
the  stability  of  the  adaptive  tracking  controller.  Simu¬ 
lation  results  are  provided  to  illustrate  the  performance 
of  the  proposed  approach. 

2  Geometric  Modeling 
2.1  Euclidean  Homography 

Four  feature  points,  denoted  by  O;  Vi  =  1,  2,  3,  4,  are 
assumed  to  be  located  on  a  reference  plane  n  (see  Figure 
1),  and  are  considered  to  be  coplanar1  and  not  colinear. 
The  reference  plane  can  be  related  to  the  coordinate 
frames  V7,  T&,  and  T*  depicted  in  Fig.  1  that  denote 
the  actual,  desired,  and  goal  pose  of  the  camera,  respec¬ 
tively.  Specifically,  the  following  relationships  can  be 
developed  from  the  geometry  between  the  coordinate 
frames  and  the  feature  points  located  on  n 

fhi  =  Xf  +  Rfh*  ,, 

nidi  =  Xfd  +  Rdfri*  ^  ’ 

where  fhi{t),  fhdi{t),  and  fh*  denote  the  Euclidean  coor¬ 
dinates  of  Ot  expressed  in  V7,  Td-,  and  V7*,  respectively. 
In  (1),  R(t),  Rd(t)  €  SO( 3)  denote  the  rotation  be¬ 
tween  T  and  T*  and  between  Td  and  V7*,  respectively, 
and  Xf  ( t ),  Xfd  ( t )  €  M3  denote  translation  vectors  from 

1  It  should  be  noted  that  if  four  coplanar  target  points  are  not 
available  then  the  subsequent  development  can  exploit  the  classic 
eight-points  algorithm  [26]  with  no  four  of  the  eight  target  points 
being  coplanar. 


T  to  T*  and  Td  to  T*  expressed  in  the  coordinates  of 
T  and  Td,  respectively.  Since  the  Euclidean  position 
of  T .  Td ,  and  T*  cannot  be  directly  measured,  the  ex¬ 
pressions  in  (1)  need  to  be  related  to  the  measureable 
image-space  coordinates.  To  this  end,  the  normalized 
Euclidean  coordinates  of  O;  expressed  in  terms  of  T , 
Td,  and  T*  as  to;  ( t ),  to*  ( t ),  to*  el3,  respectively, 
are  defined  as  follows 


TO; 


TO;  = 


A  VTldi 

mdi  = - 

Zdi 


m. 


T  (2) 


where  A  £  R3x3  is  a  known,  constant,  and  invertible 
intrinsic  camera  calibration  matrix  with  the  following 
form 

f  Or  02  04  1 


A  = 


0  0,3  05 

0  0  1 


(9) 


where  a;  £  R  Vi  =  1,  2, ...,  5,  denote  known,  constant 
calibration  parameters  (see  [13]).  After  substituting  (8) 
into  (3)  and  (4),  the  following  relationships  can  be  de¬ 
veloped 


under  the  standard  assumption  that  2;  (t),  Zdi{t ),  z*  >  e 
where  e  denotes  an  arbitrarily  small  positive  constant. 
Based  on  (2),  the  expression  in  (1)  can  be  rewritten  as 
follows 

TO;  =  —  (R  +  m* 

^  V  d*  ).  (3) 

OLi  H 

mdi  =  —  (Rd  +  TO*. 

A  d*  /,  (4) 

V^di  Rd 

In  (3)  and  (4),  ct;  (t),  aidi  (f)  €  R  denote  invertible  depth 
ratios,  H  ( t ) ,  Hd{t)  €  M3x3  denote  Euclidean  homogra- 
phies  [13],  and  d*  £  M  denotes  the  constant,  unknown 
distance  from  the  origin  of  T*  to  it.  The  following  pro¬ 
jective  relationship  can  also  be  developed  from  Fig.  1 

d*  =  n*Tm*.  (5) 

Also  from  Fig.  1,  the  unknown,  time  varying  distance 
from  the  origin  of  Td  to  7 r,  denoted  by  d  (t)  £  M,  can  be 
expressed  as  follows 

d  =  n*T  Rdfhdi-  (6) 


Pi  =  on  ( AHA  3)  p*  pdi  =  oidi  ( AHdA  3)  p\ 

■s _  _ ✓  S,  _ ✓ 

G  Gd 

(10) 

where  G(t),  Gd  (t)  £  R3x3  denote  projective  homogra- 
phies.  Given  the  images  of  the  4  feature  points  on  7r 
expressed  in  T,  Td,  and  T*.  a  linear  system  of  equa¬ 
tions  can  be  developed  from  (10).  From  the  linear  sys¬ 
tem  of  equations,  a  decomposition  algorithm  (e.g.,  the 
Faugeras  decomposition  algorithm  in  [13])  can  be  used 
to  compute  a;  (t),  o.di  (t),  n* ,  R(t),  and  Rd  (t)  (see  [5] 
for  details)2.  Hence,  a;  (f),  a^;  (t),  n* ,  R(t),  and  Rd  (t) 
are  known  signals  that  can  be  used  in  the  subsequent 
development. 

2.3  Kinematic  Model  of  Vision  System 

The  camera  pose,  denoted  by  T  (t)  £  M6,  can  be  ex¬ 
pressed  in  terms  of  a  hybrid  of  pixel  and  reconstructed 
Euclidean  information  as  follows 

(0  —  l  p!'\  eT]T  (11) 

where  the  extended  pixel  coordinate  pe  1  (t)  €  R3  is  de¬ 
fined  as  follows 

Pei  =  [  ui  vi  -ln(ai)  ]T,  (12) 


2.2  Projective  Homography 

Each  feature  point  on  n  has  a  projected  pixel  coordinate 
denoted  by  w;  (f),  u,  (t)  £  R  in  T,  Udi  (t),  Vdi  (t)  €  R  in 
Td,  and  u*,v*  el  in  T* ,  that  are  defined  as  follows 


Pi  =  [  T  Vi  1  ]  r  Pdi  —  [  Udi  vdi  1  ] ' 
p*  =  [  u*  v*  1  ] T  . 


(7) 

In  (7),  pi  ( t ),  pdi  (t),  p*  €  R3  represent  the  image-space 
coordinates  of  the  time-varying  feature  points,  the  de¬ 
sired  time- varying  feature  point  trajectory,  and  the  con¬ 
stant  reference  feature  points,  respectively.  To  calculate 
the  Euclidean  homography  given  in  (3)  and  (4)  from 
pixel  information,  the  projected  pixel  coordinates  of  the 
target  points  are  related  to  to;  (f),  nidi  (f),  and  to*  by 
the  following  pin-hole  lens  models  [13] 


Pdi  =  Airidi  p*  =  Am*  (8) 


and  0(f)  £  R3  denotes  the  following  axis-angle  repre¬ 
sentation  of  R(t)  [32] 


0  =  p(t)6{t).  (13) 

In  (12),  ln(-)  denotes  the  natural  logarithm,  and  ai(f) 
is  introduced  in  (3).  In  (13),  p,(t)  £  R3  represents  the 
unit  axis  of  rotation,  and  6  (t)  denotes  the  rotation  angle 
about  that  axis.  Based  on  the  development  in  Appendix 
A,  the  open-loop  dynamics  for  T  (f)  can  be  expressed 
as  follows 


Pel 

'  -j-Ael  Ae  1  [toi]  x 

’  vc 

0 

3 

1 

o 

U)c 

where  vc(t)  £  R3  and  ivc(t)  £  R3  denote  the  linear  and 
angular  velocity  of  the  camera  expressed  in  terms  of  T, 

-The  initial  best-guess  of  n*  can  be  utilized  to  resolve  the 
decomposition  ambiguity.  See  [6]  for  details. 


Pi  =  Ami 


Aei  ( m ,  Vi)  £  R3x3  is  a  known,  invertible  matrix  defined  PI)  Analytic  onV  (at  least  the  first  and  second  partial 
as  follows  derivatives  exist  and  are  bounded  on  V ); 


Aei  —  A 


0  0  Ui 
0  0  Vi 
0  0  0 


*  =  1,2, 3, 4,  (15) 


and  the  invertible  Jacobian-like  matrix  Lu(6,  fi)  £  R3x3 
is  defined  as 


/ 

e  r  , 

Lu  ~  I3  -  -  [p\  x  +  1 

V 


sine  (9) 


\ 


(16) 


where 


sine  ( 9  (t))  = 


sin#  (t) 


P  2)  a  unique  minimum  exists  at  p* ; 

P  3)  it  obtains  a  maximum  value  on  the  boundary  ofD 
(i.e.,  admissible  onV); 

P  4)  it  is  a  Morse  function  (i.e.,  the  matrix  of  second 
partial  derivatives,  the  Hessian,  evaluated  at  its  critical 
points  is  nonsingular  (and  has  bounded  elements  based 
on  the  smoothness  property  in  P  1)). 


3.1  Pose  Space  to  Image  Space  Relationship 

To  develop  a  desired  camera  pose  trajectory  that  en¬ 
sures  pd  (t)  £  T>,  the  desired  image  feature  vector  is 
related  to  the  desired  camera  pose,  denoted  by  (t)  £ 
R6,  through  the  following  relationship 


Remark  1  As  stated  in  [32],  the  axis-angle  representa¬ 
tion  of  (13)  is  not  unique,  in  the  sense  that  a  rotation  of 
— 9  (t)  about  —p(t)  is  equal  to  a  rotation  of  9  (t)  about 
p(t).  A  particular  solution 3  for  9(t)  and  p(t)  can  be 
determined  as  follows  [32] 


Pd  =  n(Td)  (19) 

where  II(-)  :  R6  — ►  V  denotes  an  unknown  function 
mapping  the  camera  pose  to  the  image  feature  vector4. 
In  (19),  the  desired  camera  pose  is  defined  as  follows 


(17) 

where  the  notation  tr  (•)  denotes  the  trace  of  a  matrix, 
and  [pp]  denotes  the  3x3  skew-symmetric  expansion 
of  pp(t).  From  (17),  it  is  clear  that 

0<6»p(t)<7T.  (18) 


**(*)  =  [*&  1  ej  ]T  (20) 

where  pedi  (t)  €  R3  denotes  the  desired  extended  pixel 
coordinates  defined  as  follows 

Pedi  =  [  Udi  vdi  —  ln(adi)  ]T  (21) 

where  adi(t)  is  introduced  in  (4),  and  Qd(t)  £  R3  de¬ 
notes  the  axis- angle  representation  of  Rd(t)  as  follows 


3  Image-Based  Path  Planning 


Od  =  Pd(t)9d(t)  (22) 


The  path  planning  objective  is  to  navigate  the  pose 
of  a  camera  held  by  the  end-effector  of  a  robot  ma¬ 
nipulator  to  a  desired  camera  pose  along  an  image- 
space  trajectory  that  ensures  the  target  points  re¬ 
main  visible.  To  achieve  this  objective,  a  desired 
camera  pose  trajectory  is  constructed  in  this  section 
so  that  the  desired  image  feature  vector,  denoted  by 
Pd  ( t )  =  [  Udl  ( t )  Vdl  (t)  ...  Udi  ( t )  Vdi  (' t )  ]  € 

R8,  remains  in  a  set,  denoted  by  'D  cR8,  where  all  four 
feature  points  of  the  target  remain  visible  for  a  valid 
camera  pose.  The  constant,  goal  image  feature  vector 
p*  =  [  u*  vl  ...  u%  v%  ]T  £  R8  is  assumed  be  in 
the  interior  of  T>.  To  generate  the  desired  camera  pose 
trajectory  such  that  pd(t)  £  V,  the  special  artificial  po¬ 
tential  function  coined  a  navigation  function  in  [23] ,  can 
be  used.  Specifically,  the  navigation  functions  used  in 
this  paper  are  defined  as  follows  [33]. 

Definition  1  A  map  p  (pd)  ■  P  — >[0,  1],  is  a  NF  if: 

3  See  [4]  for  further  details. 


where  Pd(t)  €  R3  and  9d(t)  £  R  are  defined  in  the  same 
manner  as  p(t)  and  9(t)  in  (13)  with  respect  to  Rd(t). 


3.2  Desired  Image  Trajectory  Planning 

After  taking  the  time  derivative  of  (19),  the  following 
expression  can  be  obtained 


Pd=Lrjd  (23) 


dp 

where  Lyd  (pd)  =  - £  R8x6  denotes  an  image 
Old 

Jacobian-like  matrix.  Based  on  the  development  in  Ap¬ 
pendix  B,  a  measurable  expression  for  Lfd  (t)  can  be 
developed  as  follows 


Lrd  =  IT  (24) 

4  The  reason  we  choose  four  feature  points  to  construct  the 
image  feature  vector  is  that  the  same  image  of  three  points  can 
be  seen  from  four  defferent  camera  poses  [20].  A  unique  camera 
pose  can  theoretically  be  obtained  by  using  at  least  four  points 
[2].  Therefore,  the  map  II  (•)  is  a  unique  mapping  with  the  image 
feature  vector  corresponding  to  a  valid  camera  pose. 


where  I  £  K.8x12  denotes  a  constant,  row-delete  matrix 
defined  as  follows 
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where  In  € 

E  M.nxn  denotes  the  n 

x  n 

identity  matrix, 

0„  £  K"x"  denotes  an  n  x  n  matrix  of  zeros,  0"  £  K" 
denotes  annxl  column  of  zeros,  and  T  (t)  £  K.12x6  is 
a  measurable  auxiliary  matrix  defined  as  follows 
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In  (25),  Ae<n  (■ Udi ,  Vdi)  £  K3x3  and  the  Jacobian- like  ma¬ 
trix  L^diPdi  Pd)  €  M3x3  are  defined  as  in  (15)  and  (16) 
with  respect  to  udi  (t) ,  vdi  (t) ,  pd  ( t ),  and  Qd  (t),  and  the 
auxiliary  variable  j3i  (t)  £  K.  is  defined  as  follows 

Pi  =  Z~f  *  =  1,2, 3, 4.  (26) 


of  I%d  ( pd )  (i-e.,  V<p(Pd)  i  NS(Lfd  (pd))  where  NS(-) 
denotes  the  null  space  operator).  However,  since  the 
approach  in  this  paper  is  developed  in  terms  of  the  de¬ 
sired  image-space  trajectory  (and  hence,  is  an  off-line 
approach),  a  particular  desired  image  trajectory  can  be 
chosen  (e.g.,  by  trial  and  error)  a  priori  to  ensure  that 
\7p(pd)  ( pd ))■  Similar  comments  are  pro¬ 

vided  in  [2]  and  [28]  that  indicate  that  in  practice  this 
assumption  can  be  readily  satisfied  for  particular  cases. 
Likewise,  a  particular  desired  image  trajectory  is  also 
assumed  to  be  a  priori  selected  to  ensure  that  Y<j(t), 
T d(t)  £  £oo  if  pd(t)  €  D.  Based  on  the  structure  of 
(20)  and  (21),  the  assumption  that  Td(t),  T d(t)  £ 
ifpd(t)  £T>  is  considered  mild  in  the  sense  that  the  only 
possible  alternative  case  is  if  the  camera  could  somehow 
be  positioned  at  an  infinite  distance  from  the  target  while 
all  four  feature  points  remain  visible. 

3.3  Path  Planner  Analysis 

Theorem  1  Provided  the  desired  feature  points  can  be 
a  priori  selected  to  ensure  that  pd  (0)  £  V  and  that 
\7p(Pd)  4-  NS(L j-d  (pd)),  then  the  desired  image  tra¬ 
jectory  generated  by  (29)  ensures  that  pd(t)  £  V  and 
(29)  has  the  asymptotically  stable  equilibrium  point  p* . 


Based  on  (2),  (6),  and  (8),  /3i(t)  can  be  rewritten  in 
terms  of  computed  and  measurable  terms  as  follows 


Pi 


1 

n*T A^1  pdi ' 


(27) 


Motivated  by  (23)  and  the  definition  of  the  navigation 
function  in  Definition  1,  the  desired  camera  pose  trajec¬ 
tory  is  designed  as  follows 


Proof:  Let  V)  (pd)  :  T>  — ►  K.  denote  a  non-negative 
function  defined  as  follows 

Vi  (pd)  =  P  (Pd)  ■  (30) 

After  taking  the  time  derivative  of  (30),  the  following 
expression  can  be  obtained 

Vi  (pd)  =  (vpf  Pd  •  (31) 


Td^-h  LxdV^  (28) 

where  k\  £  K.  denotes  a  positive  constant,  and 
V P (pd )  —  (  dVdpd  ) )  e  denotes  the  gradient  vector 
of  < p(pd )•  The  development  of  a  particular  image  space 
NF  and  its  gradient  are  provided  in  Appendix  C.  After 
substituting  (28)  into  (23),  the  desired  image  trajectory 
can  be  expressed  as  follows 

pd=-k1LrdLfCdX7iP  (29) 

where  it  is  assumed  that  \7p(pd)  is  not  a  member  of 
the  null  space  of  Lyd  (pd)-  Based  on  (23)  and  (28),  it 
is  clear  that  the  desired  image  trajectory  generated  by 
(29)  will  satisfy  rigid  body  constraints  automatically. 


Remark  2  Based  on  comments  in  [2]  and  the  current 
development,  it  seems  that  a  remaining  open  problem  is 
to  develop  a  rigorous,  theoretical  and  general  approach 
to  ensure  that  VP(Pd)  Is  not  a  member  of  the  null  space 


After  substituting  (29)  into  (31)  to  obtain  the  following 
expression 


(Pd)  —  —hi  L\d  v  p  ,  (32) 


it  is  clear  that  V\  (pfi)  is  a  non-increasing  function  in  the 
sense  that 


V!  (pd)  <  Vi  (Pdi 0)) .  (33) 


From  (30),  (33),  and  the  development  in  Appendix  C, 
it  is  clear  that  for  any  initial  condition  pd  (0)  £  T>, 
that  pd  (t)  £  V  Vf  >  0;  therefore,  V  is  a  positively 
invariant  set  [21].  Let  E\  c  T>  denote  the  following  set 
Ei  —{Pd  (f)|  Vi  (Pd)  =  0}.  Based  on  (32),  it  is  clear  that 
\\Lrd(Pd)V  <p(Pd)\\  =  0  in  Ei ;  hence,  from  (28)  and 


(29)  it  can  be  determined  that 


?d(t) 


Pd  (t) 


=  0 


in  E i,  and  that  E\  is  the  largest  invariant  set.  By  invok¬ 
ing  LaSalle’s  Theorem  [21],  it  can  be  determined  that 
every  solution  pd  (t)  €  D  approaches  E y  as  t  — ►  oo, 
and  hence,  \\Lf  d(pd)  \/ p  (pd)\\  — ►  0.  Since  pd  (t)  are 
chosen  a  priori  via  the  off-line  path  planning  routine  in 


(29),  the  four  feature  points  can  be  a  priori  selected 
to  ensure  that  \/<fi(pd)  £  AfS(Lyd  ( pd )).  Provided 

VTiPd)  i  NS(Lrd  (Pd)),  then  | \L^d(pd)  v  p(Pd)\\  =  0 
implies  that  ||v<P(Pd)ll  =  0-  Based  on  development  of 
Appendix  C,  since  \/p(pd)  — 5 ►  0  then  pd(t)  — ►  p*.  □ 

4  Tracking  Control  Development 

Based  on  Theorem  1,  the  desired  camera  pose  trajectory 
can  be  generated  from  (28)  to  ensure  that  the  cam¬ 
era  moves  along  a  path  generated  in  the  image  space 
such  that  the  desired  object  features  remain  visible  (i.e., 
pd(t)  £  V).  The  objective  in  this  section  is  to  de¬ 
velop  a  controller  so  that  the  actual  camera  pose  T  ( t ) 
tracks  the  desired  camera  pose  ( t )  generated  by  (28), 
while  also  ensuring  that  the  object  features  remain  visi¬ 
ble  (i.e.,  p(t)  =  [  ui(t)  v\  (t)  ...  U4  (f)  V4  (t)  ]T  £ 

T>).  To  quantify  this  objective,  a  rotational  tracking  er¬ 
ror,  denoted  by  eu(t)  €  R3,  is  defined  as 

ew  =  Q-  Gd,  (34) 

and  a  translational  tracking  error,  denoted  by  ev  ( t )  £ 
R3,  is  defined  as  follows 

€v  =  Pel  Pedl'  (3b) 

4.1  Control  Development 

After  taking  the  time  derivative  of  (34)  and  (35),  the 
open-loop  dynamics  for  ew  (f)  and  ev  ( t )  can  be  obtained 
as  follows 

&uj  Gd  (36) 

ev  =  ~—Ae ivc  +  Ae i  [mi]x  ujc  -  pedi  (37) 

%i 

where  (14)  was  utilized.  Based  on  the  open-loop  error 
systems  in  (36)  and  (37),  vc(t)  and  eoc(t)  are  designed 
as  follows 

cnc  4  L~ 1  (Afyew  -  0d)  (38) 

vc  =  —  A~l  (Kvev  -  z*pedi)  +  —  [mi]  t ocz{  (39) 

Oi  O  i 

where  Afy,  Kv  £  R3x3  denote  diagonal  matrices  of  pos¬ 
itive  constant  control  gains,  and  z((t)  £  R  denotes  a 
parameter  estimate  for  zf  that  is  designed  as  follows 
.  >(< 

Zl-  k2ev  {Ae  1  [toi]x  U)c  -Pedl)  (40) 

where  k2  £  ^  denotes  a  positive  constant  adaptation 
gain.  After  substituting  (38)  and  (39)  into  (36)  and 
(37),  the  following  closed- loop  error  systems  can  be  de¬ 
veloped 

A^jCq;  (44) 

Z\CV  Rv&v  "f  (Ael  [mi]x  LV c  Pedl)  Z\  (42) 

where  the  parameter  estimation  error  signal  z*(t)  £  R 
is  defined  as  follows 

K  =  zl-zl.  (43) 


4.2  Controller  Analysis 

Theorem  2  The  controller  introduced  in  (38)  and 
(39),  along  with  the  adaptive  update  law  defined  in  (40), 
ensure  that  the  actual  camera  pose  tracks  the  desired 
camera  pose  trajectory  in  the  sense  that 

||ew(f)||  — *  0  ||e„(t)||  — ►  0  as  t  — ►  oo.  (44) 

Proof:  Let  V2(t)  £  R  denote  a  non-negative  function 
defined  as  follows 

V2  =  ^eu  +  |e^  +  ^i*2.  (45) 

After  taking  the  time  derivative  of  (45)  and  then  sub¬ 
stituting  for  the  closed-loop  error  systems  developed  in 
(41)  and  (42),  the  following  expression  can  be  obtained 

L2  —  evKvev  , 

+  e()  (Ael  [mi]x  lvc  -pedi)  z(  -  ^z{  z* 

where  the  time  derivative  of  (43)  was  utilized.  After 
substituting  the  adaptive  update  law  designed  in  (40) 
into  (46) ,  the  following  simplified  expression  can  be  ob¬ 
tained 

V2  =  -eO  Afy  e^-eO  Kv  ev.  (47) 

Based  on  (43),  (45),  and  (47),  it  can  be  determined  that 

eu(t),  ev(t),  z*(t),  z*(t)  £  Coo  and  that  ew(t),  ev(t)  £ 

L 2 ■  Based  on  the  assumption  that  Gd(t)  is  bounded 

(see  Remark  2),  the  expressions  given  in  (34),  (38),  and 

Lu  ( t )  in  (16)  can  be  used  to  conclude  that  uc(t)  £  Too. 

Since  ev(t)  £  C^,  (35),  (12),  (8),  and  Ae\(t)  in  (15) 

can  be  used  to  prove  that  ui(t),  14  (t),  04(f),  mi(f), 

Aei(t)  £  Coo-  Based  on  the  assumption  that  pedi(t)  is 

bounded  (see  Remark  2),  the  expressions  in  (39),  (40), 

.  * 

and  (42)  can  be  used  to  conclude  that  vc(t),  zx  (t), 
ev(t)  £  Too-  Since  eu(t)  £  C^,  it  is  clear  from  (41)  that 
eu(t)  £  Too.  Since  ew(f),  e„(f)  €  C2  and  ew(f),  ew(t), 
ev(t),  ev(t)  £  Coo,  Barbalat’s  Lemma  [31]  can  be  used 
to  prove  the  result  given  in  (44).  □ 

Remark  3  Based  on  the  result  provided  in  (44),  it  can 
he  proven  from  the  Euclidean  reconstruction  given  in 
(3)  and  (4)  that  R(t)  — ►  Rd(t),  mfyt)  — ►  m.di(t),  and 
zi(t)  — ►  Zdi(t)  (and  hence,  Xf(t)  — ►  Xfd(t)).  Based 
on  these  results,  (1)  can  he  used  to  also  prove  that 
fhfit)  — ►  fhdfit).  Since  II  (•)  is  a  unique  mapping,  we 
can  conclude  that  the  desired  camera  pose  converges 
to  the  goal  camera  pose  based  on  the  previous  result 
Pd(t)  — >  p*  from  Theorem  1.  Based  on  the  above  analy¬ 
sis,  rhift)  — ►  fh*. 

Remark  4  Based  on  (45)  and  (47),  the  following  in¬ 
equality  can  be  obtained 

e^ew  +  e^e„  <  2maxjl,-^j  V2(t)  (48) 


2  max 


U2(0) 


by  the  feature  points)  from  a  practical  point  of  view,  we 
define  a  distance  ratio  7  (t)  gR  as  follows 


where 

^2  (0)  =  ^e^'(0)ea,(0)  +  ye^(0)e„(0)  + 

From  (11),  (20),  (34),  (35),  and  the  inequality  in  (48), 
the  following  inequality  can  be  developed 


||T-Td|K  W2max|l,^|y2(0).  (49) 


7(*)  =  ^  (54) 

From  [25] ,  7  (t)  is  measurable.  The  idea  to  avoid  the 
self-occlusion  is  to  plan  a  desired  image  trajectory  with¬ 
out  self-occlusion.  Based  on  (52),  we  can  assume  that 
the  actual  trajectory  is  close  enough  to  the  desired  tra¬ 
jectory  such  that  no  self-occlusion  occured  for  the  actual 
trajectory.  This  assumption  has  been  verified  by  many 
simulation  trials. 


Based  on  (19),  the  following  expression  can  be  developed 

p  =  n(T)-n(Td)+pd.  (50) 

After  applying  the  mean-value  theorem  to  (50),  the  fol¬ 
lowing  inequality  can  be  obtained 


To  illustrate  the  performance  of  the  path  planner  given 
in  (53)  and  the  controller  given  in  (38)-(40),  we  imple¬ 
mented  the  simulations  for  four  standard  visual  servo 
tasks,  which  are  believed  represent  the  most  interesting 
tasks  encountered  by  visual  servo  system  [18]: 


M\<  I|£tJ  ||T-Td||  +  \\pd\\-  (51) 

Since  all  signals  are  bounded,  it  can  be  shown  that 
Lr d(Pd)  €  £00/  hence,  the  following  inequality  can  be 
developed  from  (49)  and  (51) 

I|P||<C6V^(0)  +  W  (52) 

for  some  positive  constant  €  K,  where  pd  (t)  €  D 
based  on  Theorem  1.  To  ensure  that  p(t)  €  V,  the 
image  space  needs  to  be  sized  to  account  for  the  effects 
o/Cb^2(0).  Based  on  ,  V2  (0)  can  be  made  arbitrarily 
small  by  increasing  k-2  and  initializing  pd  (0)  close  or 
equal  to  p  (0) . 


5  Simulation  Results 

From  a  practical  point  of  view,  we  choose  a  state-related 
time  varying  control  gain  matrix  (Lj-L~c)  instead 
of  a  constant  k\  in  (28)  for  the  image  path  planner  as 
follows 

T<2  =  —  fc3  ( LyL x)  L'Td  V  T  (53) 

where  fc3  €  K.  is  a  constant  control  gain.  Through  many 
simulation  trials,  we  conclude  that  the  path  planner  in 
(53)  works  better  than  the  path  planner  in  (28).  Using 
the  path  planner  in  (53)  instead  of  the  path  planner  in 
(28)  will  not  affect  the  proof  for  Theorem  1  as  long  as 
Lff-Lx  is  positive  definite  along  the  desired  image  tra¬ 
jectory  pd  ( t )  (It  is  clear  that  Lj-Ly  is  positive  definite 
if  Lyd  (Pd)  is  full  rank).  Similar  to  the  statement  in 
Remark  2,  this  assumption  is  readily  satisfied  for  this 
off-line  path  planner  approach. 

To  solve  the  self-occlusion  problem  (The  terminology, 
self-occlusion,  in  this  paper  is  utilized  to  denote  the  case 
when  the  center  of  the  camera  is  in  the  plane  determined 


•  Task  1:  Optical  axis  rotation,  a  pure  rotation  about 
the  optic  axis 

•  Task  2:  Optical  axis  translation,  a  pure  translation 
along  the  optic  axis 

•  Task  3:  Camera  y-axis  rotation,  a  pure  rotation  of 
the  camera  about  the  y-axis  of  the  camera  coordi¬ 
nate  frame. 

•  Task  4:  General  camera  motion,  a  transformation 
that  includes  a  translation  and  rotation  about  an 
arbitrary  axis. 


For  the  simulation,  the  intrinsic  camera  calibration  ma¬ 
trix  is  given  as  follows 


A  = 


"  fku 

0 

0 


-  fku  cot  f) 

fkv 

sin  (j) 

0 


u0 

Vo 

1 


(55) 


where  uo  =  257  [pixels],  vo  =  253  [pixels],  ku  =  101.4 
[pixels-mm_1]  and  kv  =  101.4  [pixels-mm-1]  represent 
camera  scaling  factors,  </>  =  90  [Deg]  is  the  angle  be¬ 
tween  the  camera  axes,  and  /  =  12.5  [mm]  denotes  the 
camera  focal  length. 


5.1  Simulation  Results:  Optical  axis  rotation 

The  initial  desired  image-space  coordinates  and  the  ini¬ 
tial  desired  image-space  coordinates  of  the  4  target 
points  were  selected  as  follows  (in  pixels) 

Pi  (0)  =  Pdi  (0)  =  [  434  445  1  1 T 

P2  (0)  =  Pd2  (0)  =  [  56  443  1  V 

P3  (0)  =  pd3  (0)  =  [  69  49  1  ]f 

P4  (0)  =  PdA  (0)  =  [  449  71  1  ]T 
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Figure  2:  Desired  Image  Trajectory  of  Task  1 


while  the  image-space  coordinates  of  the  4  constant  ref¬ 
erence  target  points  were  selected  as  follows  (in  pixels) 


p\  =  [  416  46  1  ] T  pl=[  479  418  1 
pi  =  [  88  473  1  1 T  p%  =  [  45  96  1  ] 


(56) 

The  feedback  gains  Kv  and  Ku ,  the  adaptation  gain  k-2 
and  control  gain  k 3,  and  the  image  navigation  function 
parameter  K  and  n  were  adjusted  through  trial  and 
error  to  the  following  values  to  yield  improved  perfor¬ 
mance 


Kv  =  diag  {1, 1, 1}  Ku  =  diag{0.3, 0.3, 0.3} 

k2  =  0.04  k3  =  400000  k  =  8 
K  =  diag  {10, 10, 10, 18, 13, 15, 10, 10}  . 


The  resulting  desired  image  trajectory  and  actual  im¬ 
age  trajectory  are  depicted  in  Figure  2  and  Figure  3, 
translational  and  rotational  errors  of  the  target  are  de¬ 
picted  in  Figure  4  and  Figure  5,  respectively,  and  the 
parameter  estimate  signal  is  depicted  in  Figure  6.  The 
control  input  velocities  u>c{t)  and  vc{t)  defined  in  (38) 
and  (39)  are  depicted  in  Figure  8  and  7.  From  Figure  2 
and  Figure  3,  it  is  clear  that  the  desired  feature  points 
and  actual  feature  points  remain  in  the  camera  field  of 
view  and  converge  to  the  goal  feature  points.  Figure  4 
and  Figure  5  show  that  the  tracking  errors  go  to  zero 
as  t  — >  00.  From  Figure  4  to  Figure  8,  it  is  clear  that 
all  the  signals  are  bounded. 


5.2  Simulation  Results:  Optical  axis  translation 

The  initial  desired  image-space  coordinates  and  the  ini¬ 
tial  desired  image-space  coordinates  of  the  4  target 
points  were  selected  as  follows  (in  pixels) 

Pi  (0)  =  Pdi  (0)  =  [  363  115  1  ]T 

P2  (0)  =  pd2  (0)  -  [  402  361  1  ]T 

Ps  (0)  =  Pd3  (0)  =  [  147  397  1  ]T 

P4  (0)  =  PdA  (0)  =  [  116  148  1  ]T 


Figure  3:  Actual  Image  Trajectory  of  Task  1 


Figure  4:  Translational  Error  of  Task  1 


Time  [s] 


Figure  5:  Rotational  Error  of  Task  1 
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Figure  6:  Estimate  of  z\  of  Task  1 


Figure  7:  Linear  Velocity  Input  of  Task  1 


Time  [s] 


Figure  8:  Angular  Velocity  of  Task  1 


Figure  9:  Desired  Image  Trajectory  of  Task  2 


while  the  image-space  coordinates  of  the  4  constant  ref¬ 
erence  target  points  were  selected  as  follows  (in  pixels) 


p\  =  [  416  46  1  ]T  p*2=[  479  418  1 
pi  =  [  88  473  1  }T  p%=[  45  96  1  ] 


The  feedback  gains  K v  and  Ku ,  the  adaptation  gain  k2 
and  control  gain  ks,  and  the  image  navigation  function 
parameter  K  and  n  were  adjusted  through  trial  and 
error  to  the  following  values  to  yield  improved  perfor¬ 
mance 


Kv  =  diagjl,  1, 1}  Ku  =  diag  {0.3, 0.3, 0.3} 
k2  =  0.0004  k3  =  10000  k  =  8 
K  =  diag  {30, 20, 10, 28, 33, 25, 10, 40}  . 


The  resulting  desired  image  trajectory  and  actual  im¬ 
age  trajectory  are  depicted  in  Figure  9  and  Figure  10, 
translational  and  rotational  errors  of  the  target  are  de¬ 
picted  in  Figure  11  and  Figure  12,  respectively,  and  the 
parameter  estimate  signal  is  depicted  in  Figure  13.  The 
control  input  velocities  wc{t)  and  vc(t)  defined  in  (38) 
and  (39)  are  depicted  in  Figure  15  and  14.  From  Fig¬ 
ure  9  and  Figure  10,  it  is  clear  that  the  desired  feature 
points  and  actual  feature  points  remain  in  the  camera 
field  of  view  and  converge  to  the  goal  feature  points. 
Figure  11  and  Figure  12  show  that  the  tracking  errors 
go  to  zero  as  t  — ►  oo.  From  Figure  11  to  Figure  15,  it  is 
clear  that  all  the  signals  are  bounded. 


5.3  Simulation  Results:  Camera  y-axis  rotation 

The  initial  desired  image-space  coordinates  and  the  ini¬ 
tial  desired  image-space  coordinates  of  the  4  target 
points  were  selected  as  follows  (in  pixels) 

Pi  (0)  =  Pdi  (0)  =  [  98  207  1  ]T 
P2  (0)  =  Pd2  (0)  —  [  112  288  1  1 T 

P3  (0)  =  pd3  (0)  =  [  29  301  1  ] f 
Pa  (0)  =  PdA  (0)  =  [  15  217  1  ]r 
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Figure  10:  Actual  Image  Trajectory  of  Task  2 


Figure  13:  Estimate  of  of  Task  2 


Time  [s] 


Figure  11:  Translational  Error  of  Task  2 


Figure  14:  Linear  Velocity  Input  of  Task  2 


Figure  12:  Rotational  Error  of  Task  2 


Figure  15:  Angular  Velocity  Input  of  Task  2 
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Figure  16:  Desired  Image  Trajectory  of  Task  3 

while  the  image-space  coordinates  of  the  4  constant  ref¬ 
erence  target  points  were  selected  as  follows  (in  pixels) 

p\  =  [  478  206  1  ] T  pl=[  492  289  1  ] T 

pi  =  [  408  300  1  ]T  pt=[  395  218  1  ]T . 

The  feedback  gains  Kv  and  Kw ,  the  adaptation  gain  k2 
and  control  gain  £:3,  and  the  image  navigation  function 
parameter  K  and  k  were  adjusted  through  trial  and 
error  to  the  following  values  to  yield  improved  perfor¬ 
mance 

Kv  =  diagjl,  1, 1}  Ku  =  diag  {0.3, 0.3, 0.3} 

k2  =  0.0004  k2  =  1000000  k  =  8 
K  =  diag  {30, 20, 10, 28, 33, 25, 10, 40}  . 

The  resulting  desired  image  trajectory  and  actual  im¬ 
age  trajectory  are  depicted  in  Figure  16  and  Figure  17, 
translational  and  rotational  errors  of  the  target  are  de¬ 
picted  in  Figure  18  and  Figure  19,  respectively,  and  the 
parameter  estimate  signal  is  depicted  in  Figure  20.  The 
control  input  velocities  u>c{t)  and  vc{t)  defined  in  (38) 
and  (39)  are  depicted  in  Figure  22  and  21.  From  Fig¬ 
ure  16  and  Figure  17,  it  is  clear  that  the  desired  feature 
points  and  actual  feature  points  remain  in  the  camera 
field  of  view  and  converge  to  the  goal  feature  points. 
Figure  18  and  Figure  19  show  that  the  tracking  errors 
go  to  zero  as  t  — ►  oo.  From  Figure  18  to  Figure  22,  it  is 
clear  that  all  the  signals  are  bounded. 

5.4  Simulation  Results:  General  Camera  Mo¬ 
tion 

The  initial  desired  image-space  coordinates  and  the  ini¬ 
tial  desired  image-space  coordinates  of  the  4  target 
points  were  selected  as  follows  (in  pixels) 

Pi  (0)  =  pdi  (0)  =  [  267  428  1  ]T 

P2  (0)  =  Pd2  (0)  —  [  295  257  1  ]T 

Pi  (0)  =  Pdi  (0)  =  [  446  285  1  ]T 

Pi  (0)  =  PdA  (0)  —  [  420  449  1  ]T 


Figure  17:  Actual  Image  Trajectory  of  Task  3 


Time  [s] 


Figure  18:  Translational  Error  of  Task  3 


Time  [s] 
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Figure  19:  Rotational  Error  of  Task  3 


Figure  23:  Desired  Image  Trajectory  of  Task  4 


Figure  20:  Estimate  of  of  Task  3 


Figure  21:  Linear  Velocity  Input  of  Task  3 


Figure  22:  Angular  Velocity  Input  of  Task  3 


while  the  image-space  coordinates  of  the  4  constant  ref¬ 
erence  target  points  were  selected  as  follows  (in  pixels) 


p\  =  [  416  46  1  ]T  p*2  =  [  479  418  1 
pi  =  [  88  473  1  ] T  p%  =  [  45  96  1  ] 


The  feedback  gains  Kv  and  Ku ,  the  adaptation  gain  k2 
and  control  gain  k3,  and  the  image  navigation  function 
parameter  K  and  k  were  adjusted  through  trial  and 
error  to  the  following  values  to  yield  improved  perfor¬ 
mance 


Kv  =  diagjl,  1, 1}  Ku  =  diag  {0.3, 0.3, 0.3} 

k2  =  0.004  k3  =  200000  k  =  8 
K  =  diag  {10, 10, 10, 18, 13, 15, 10, 10}  . 


The  resulting  desired  image  trajectory  and  actual  im¬ 
age  trajectory  are  depicted  in  Figure  23  and  Figure  24, 
translational  and  rotational  errors  of  the  target  are  de¬ 
picted  in  Figure  25  and  Figure  26,  respectively,  and  the 
parameter  estimate  signal  is  depicted  in  Figure  27.  The 
control  input  velocities  uc{t)  and  vc(t)  defined  in  (38) 
and  (39)  are  depicted  in  Figure  29  and  28.  From  Fig¬ 
ure  23  and  Figure  24,  it  is  clear  that  the  desired  feature 
points  and  actual  feature  points  remain  in  the  camera 
field  of  view  and  converge  to  the  goal  feature  points. 
Figure  25  and  Figure  26  show  that  the  tracking  errors 
go  to  zero  as  t  — ►  oo.  From  Figure  25  to  Figure  29,  it  is 
clear  that  all  the  signals  are  bounded. 


6  Conclusions 

A  path  planner  is  developed  based  on  an  image-space 
NF  that  ensures  the  desired  image  trajectory  converges 
to  the  goal  position  while  also  ensuring  the  desired  im¬ 
age  features  remain  in  a  visibility  set  under  certain  tech¬ 
nical  restrictions.  An  adaptive,  homography-based  vi¬ 
sual  servo  tracking  controller  is  then  developed  to  nav¬ 
igate  the  camera-in-hand  pose  along  the  desired  tra¬ 
jectory  despite  the  lack  of  depth  information  from  a 
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Figure  24:  Actual  Image  Trajectory  of  Task  4 


Figure  27:  Estimate  of  z\  of  Task  4 


Figure  26:  Rotational  Error  of  Task  4 


Figure  29:  Angular  Velocity  Input  of  Task  4 


monocular  camera  system.  The  path  planner  and  the 
tracking  controller  are  analyzed  through  a  Lyapunov- 
based  analysis.  Simulation  results  are  provided  to  illus¬ 
trate  the  performance  of  the  proposed  approach. 
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Appendix  A 

The  extended  image  coordinates  pe i  (t)  of  (12)  can  be 
written  as  follows 


a\  a  2  0 

-  aq 

Zl 

a 4 

Pel  = 

0  03  0 

0  0  1 

V\ 

Zl 

.  In  (zi)  _ 
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.  -In  (4)  . 

(57) 

where  (7),  (8),  and  (9)  were  utilized.  After  taking  the 
time  derivative  of  (57),  the  following  expression  can  be 
obtained 

1  ,  . 

Pel  =  —Ael  TOi  . 

Zi 

By  exploiting  the  fact  that  fhi  ( t )  can  be  expressed  as 
follows 

fhi=  -vc  +  [mi]x  ujc, 

the  open- loop  dynamics  for  pe\(t )  can  be  rewritten  as 
follows 

Pel  = - A !VC  +  Ae i  [mi]  <jc. 

z  i 

The  open-loop  dynamics  for  0(f)  can  be  expressed  as 
follows  [11] 

0  Lwujc. 

Appendix  B 

Similar  to  (14),  the  dynamics  for  T d  (t)  can  be  expressed 
as 


Pedi 

Zd  1  A?dl 

■A-edl  [^dl]  x 

Vcd 
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Lojd 

V cd 

(58) 

where  Qd(t)  is  defined  in  (22),  Zdi(t)  is  introduced  in 
(2),  Aein(iidi,Vdi)  is  defined  in  the  same  manner  as 
in  (15)  with  respect  to  the  desired  pixel  coordinates 
Udi(t),Vdi(t ),  mdi(t)  is  given  in  (1),  Lud(6d,g,d)  is  de¬ 
fined  in  the  same  manner  as  in  (16)  with  respect  to  9d(t) 
and  gd(t),  and  vcd(t),  u>cd(t)  €  K3  denote  the  desired 
linear  and  angular  velocity  signals  that  ensure  compat¬ 
ibility  with  (58).  The  signals  vcd(t)  and  ucd(t)  are  not 
actually  used  in  the  trajectory  generation  scheme  pre¬ 
sented  in  this  paper  as  similarly  done  in  [5] ;  rather,  these 


signals  are  simply  used  to  clearly  illustrate  how  pd  (f) 
can  be  expressed  in  terms  of  T d(t)  as  required  in  (23). 
Specifically,  we  first  note  that  the  top  block  row  in  (58) 
can  be  used  to  write  the  time  derivative  of  ped2(t)  in 
terms  of  vcd(t)  and  u>cd(t)  with  i  =  2 


Ped2  =  [  —  ~^Aed2  Aed 2  [rrid2}x 


vcd 

V cd 


(59) 


where  pedi(t)  is  defined  in  the  same  manner  as  (21)  Vi  = 
1,  2,  3,  4.  After  inverting  the  relationship  given  by  (58), 
we  can  also  express  vcd(t)  and  ucd(t)  as  a  function  of 
T d(t)  as  follows 


Vcd 
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(Vcd 
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Li 

After  substituting  (60)  into  (59),  ped2(t)  can  be  ex¬ 
pressed  in  terms  of  T d(t)  as  follows 

Ped2  =  [  f ^AdzAdl  Aed2  [j^mdl  ~  ™d2]  ^  L~\  J  f d. 

(61) 

After  formulating  similar  expressions  for  ped3(t)  and 
Pediit)  as  the  one  given  by  (61)  for  ped2(t),  we  can  com¬ 
pute  the  expression  for  Lrd  (pd)  in  (24)  by  utilizing  the 
definitions  of  pdi  (t)  and  pedi  (f)  given  in  (7)  and  (21), 
respectively  (i.e.,  we  must  eliminate  the  bottom  row  of 
the  expression  given  by  (61)). 


Appendix  C 


Inspired  by  the  framework  developed  in  [9],  an  image 
space  NF  is  constructed  by  developing  a  diffeomor- 
phism5  between  the  image  space  and  a  model  space, 
developing  a  model  space  NF,  and  transforming  the 
model  space  NF  into  an  image  space  NF  through  the 
diffeomorphism  (since  NFs  are  invariant  under  diffeo- 
morphism  [23]).  To  this  end,  a  diffeomorphism  is  de¬ 
fined  that  maps  the  desired  image  feature  vector  pd  to 
the  auxiliary  model  space  signal  (  (pd)  =  [£x  (pd)  (2  (pd) 
...  Cs  (Pd)]T  '■  [—  1,  l]8  — >  R8  as  follows 

c  =  diag{- — ,  - — 3^— — hg~}Pd( 62) 

“max  “min  “max  t'min  “max  “min 

_  I"  44 max  ~j~4Xm  41max  H—^min  ilmaxillmin  1  ^ 

L  4imax  44  min  41max  41min  41max  41min  J 

In  (62),  wmax,  umin,  “max,  and  umin  e  K.  denote  the 
maximum  and  minimum  pixel  values  along  the  u  and 
v  axes,  respectively.  The  model  space  NF,  denoted  by 
ip  (C)  €  K8  — ►  K,  is  defined  as  follows  [9] 


y(C)  = 


1  A  ip' 


(63) 


5  A  diffeomorphism  is  a  map  between  manifolds  which  is  dif¬ 
ferentiable  and  has  a  differentiable  inverse. 
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In  (63),  tp  (C)  €  K8  — »  R  is  defined  as 

P(0  -  \f  (C)T  Kf  (C)  (64) 


where  the  auxiliary  function  /(C)  :  (— 1,1)8  — *  K8  is 
defined  similar  to  [9]  as  follows 


/( 0  = 


Cl  -ct 
(i-Cff/2‘ 


Cs  Cs 

(wr)1/2' 


(65) 


where  A'  g  l8x8  is  a  positive  definite,  symmetric  ma¬ 
trix,  and  k  is  a  positive  parameter.  The  reason  we  use 
k  instead  of  1  as  in  [9]  is  to  get  an  additional  parameter 
to  change  the  potential  field  formed  by  /(C)-  See  [9] 
for  a  proof  that  (63)  satisfies  the  properties  of  a  NF  as 
described  in  Definition  1.  The  image  space  NF,  denoted 
by  <p  (Pd)  G  V  — ►  K,  can  then  be  developed  as  follows 


V  (Pd)  =  </  °  C  ( Pd ) 


(66) 


where  o  denotes  the  composition  operator.  The  gradient 
vector  \/<p  {pd)  can  be  expressed  as  follows 


V<P  : 


/  difi_ \T  (  dip  dC  \  T 


\dpd 


dC  dpd  J 


(67) 


In  (67),  the  partial  derivative  expressions  9<g^^ ,  9%^ , 
and  dg^'>  can  be  expressed  as  follows 


wrdms{ 


Umax  l^min  %ax 


max  ymin 


b’max  Ilmin 


dip 


-fTK^~ 

a/- 


d(  [l  +  p)2J 


}  (68) 


(69) 


df  J.  f  i-Cf'Vl 

—  =  diag  t 


1  /-“2k  —  1/-* 
1  S8  S8 


d(^  a  ^  (2k+1)/2k  ’  5  ^  _  ^.2k,^  (2k+1)/2k 

’  (70) 

It  is  clear  from  (62)-(70)  that  pd(t)  — >  p*  when 
VP  (Pd)  -*•  o. 
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